package dipl.algorithm.math.geometry.op;

import dipl.algorithm.math.geometry.BoundingWedge2d;
import dipl.algorithm.math.utility.ApfloatUtils;
import dipl.algorithm.math.utility.FPMath;
import dipl.algorithm.math.primitive.Side;
import dipl.algorithm.math.primitive.Vector2d;
import dipl.algorithm.math.primitive.op.ComplexOps;
import dipl.algorithm.math.primitive.op.Vector2dOps;
import org.apfloat.Apfloat;

/**
 * Operations on 2d-cones
 */
public class BoundingWedge2dOps {

	//
	// PUBLIC METHODS
	//

	/**
	 * Returns whether given two bounding wedges overlap (when positioned at point (0,0)).
	 * Touching wedges do overlap!
	 * Equal degenerate cones do overlap!
	 * @param fpMath
	 * @param w1 a wedge
	 * @param w2 a wedge
	 * @return
	 */
	public static boolean Overlap( FPMath fpMath, BoundingWedge2d w1, BoundingWedge2d w2 ) {
    // handle special case when both cones are degenerated
		if( w1.angle.compareTo( Apfloat.ZERO ) == 0 && w2.angle.compareTo( Apfloat.ZERO ) == 0 ) {
			return (Vector2dOps.WhichSide( w1.left, w2.left ) == Side.Onto);
		}
		// if one of the opening angles is greater or equal 180° then both overlap!
		if( !(w1.angle.compareTo( fpMath.PI ) < 0) || !(w2.angle.compareTo( fpMath.PI ) < 0) ) {
			return true;
		}
		// otherwise both angles  180° and both wedges are not degenerated

    // calculate angles of 
    Apfloat angle_l1a = ComplexOps.Argument( fpMath, w1.left.x, w1.left.y );
		Apfloat angle_r1a = fpMath.m.subtract( angle_l1a, w1.angle ); boolean o1 = angle_r1a.compareTo( Apfloat.ZERO ) < 0;

    Apfloat angle_l2 = ComplexOps.Argument( fpMath, w2.left.x, w2.left.y );
		Apfloat angle_r2 = fpMath.m.subtract( angle_l2, w2.angle ); boolean o2 = angle_r2.compareTo( Apfloat.ZERO ) < 0;

    // test overlapping between first cone of first wedge and first cone of second wedge
		if( o1 ^ o2 ) {
			if( o1 ) angle_r1a = fpMath.m.add( angle_r1a, fpMath._2PI );
			if( o2 ) angle_r2 = fpMath.m.add( angle_r2, fpMath._2PI );
			if( angle_r1a.compareTo( angle_l2 ) <= 0 || angle_r2.compareTo( angle_l1a ) <= 0 )
        return true;
		}
		else {
			if( angle_r1a.compareTo( angle_l2 ) <= 0 && angle_r2.compareTo( angle_l1a ) <= 0 )
        return true;
		}

    Apfloat angle_l1b = fpMath.m.add( angle_l1a, fpMath.PI );
    if( angle_l1b.compareTo( fpMath._2PI ) >= 0 ) angle_l1b = fpMath.m.subtract( angle_l1b, fpMath._2PI );
		Apfloat angle_r1b = fpMath.m.subtract( angle_l1b, w1.angle ); o1 = angle_r1b.compareTo( Apfloat.ZERO ) < 0;

     // test overlapping between second cone of first wedge and first cone of second wedge
		if( o1 ^ o2 ) {
			if( o1 ) angle_r1b = fpMath.m.add( angle_r1b, fpMath._2PI );
			if( angle_r1b.compareTo( angle_l2 ) <= 0 || angle_r2.compareTo( angle_l1b ) <= 0 )
        return true;
		}
		else {
			if( angle_r1b.compareTo( angle_l2 ) <= 0 && angle_r2.compareTo( angle_l1b ) <= 0 )
        return true;
		}

    return false;
	}

	/**
	 * Returns union of given two bounding wedges.
	 * The left bounding vector of the result is the vector with greater angle
	 *
	 * The opening angle of the resulting wedge is n between [0,2*pi]
	 * @param fpMath
	 * @param w1
	 * @param w2
	 * @return
	 */
	public static BoundingWedge2d Union( FPMath fpMath, BoundingWedge2d w1, BoundingWedge2d w2 ) {
		// determine left bounding vector
		Apfloat phi1 = ComplexOps.Argument( fpMath, w1.left.x, w1.left.y );
		Apfloat phi2 = ComplexOps.Argument( fpMath, w2.left.x, w2.left.y );
		Apfloat angle;
		Vector2d left;
		if( phi1.compareTo( phi2 ) >= 0 ) { // first cone lies left of second cone?
			left = new Vector2d( w1.left );
			angle = fpMath.m.subtract( phi1, w1.angle ).compareTo( fpMath.m.subtract( phi2, w2.angle ) ) < 0
							? w1.angle : fpMath.m.subtract( phi1, fpMath.m.subtract( phi2, w2.angle ) );
		}
		else { // second cone lies left of first
			left = new Vector2d( w2.left );
			angle = fpMath.m.subtract( phi2, w2.angle ).compareTo( fpMath.m.subtract( phi1, w1.angle ) ) < 0
							? w2.angle : fpMath.m.subtract( phi2, fpMath.m.subtract( phi1, w1.angle ) );
		}
		return new BoundingWedge2d( left, ApfloatUtils.Min( angle, fpMath._2PI ) );
	}
}
